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ABSTRACT 


The effects of the state excitation matrix Q x in the smoothing routine of an extended 
Kalman filter is investigated. A new algorithm to derive the 0 K matrix is also developed. 
In addition, the accuracy of the filter was substantially improved by implementing a new 
maneuver detection technique Several tracking scenarios are simulated and analyzed for 
noise free and noisy cases and statistical data are obtained for the maneuver detection 
technique. The program codes are included as appendices. 
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THESIS DISCLAIMER 


The reader is cautioned that computer programs developed in this research may not 
have been exercised for all cases of interest. While every effort has been made, within 
the time available, to ensure that the programs are free of computational and logic er¬ 
rors, they cannot be considered validated. Any application of these programs without 
additional verification is at the risk of the user. 
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I. INTRODUCTION 


During the last century, man has reached out over ever-increasing distances. 
Manmade devices have been sent beyond our solar system and to the deepest points of 
the oceans. These recent developments have focused new attention on an existing 
problem: how to accurately track long-range devices along their voyages in unknown 
environments. This problem is made even worse when only passive sensors can be used. 
One particular problem applicable to naval technology is tracking a ship by lines of 
bearing obtained by passive sensors. A powerful method of dealing with this problem, 
known as Kalman filtering, has been used with great success since Kalman and Bucy 
[Refs. 1,2] first presented its principles 30 years ago. 

This report further develops an existing Kalman filter to which a fixed-interval 
smoothing algorithm has been added. In this research, we examine how the overall ac¬ 
curacy of the extended Kalman filter is affected by applying a noise process in the 
smoothing algorithm. We also develop a new maneuver detection technique and study 
how the filter performs when using it. This research is based on previous work done by 
Lieutenant Thomas K. Bennett [Ref. 3] and Lieutenant William J. Galinis [Ref. 4], 
They investigated the problems of two ships tracking a third only by passive radio di¬ 
rection finding equipment. 

This report is organized into six major sections. The first section is this introduc¬ 
tion, which serves as a guide to approaching this report. In Chapter II, the physical 
tracking system used for simulations in this report is modeled. Chapter III gives the 
basic principles of the Kalman filtering and fixed-interval smoothing. In Chapter IV, 
we investigate how the noise process in the smoothing routine and a new maneuver de¬ 
tection technique affect the accuracy of the extended Kalman filter, t apters V and VI 
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show the simulations and present the conclusions. The appendices list the program 
codes used in this research. 


2 





PROBLEM STATEMENT 


A. THE SYSTEM MOL L 

The system used in this thesis includes two sensors and one target ship. A two- 
dimensional cartesian coordinate system is used, in which the positive x and positive y 
directions correspond to East and North, respectively. The target and sensor ships are 
both free to move throughout this coordinate space. For simplicity, the following as¬ 
sumptions are made during the development of this model: [Ref. 4] 

• The effect of the wind, current and other forces on the ship are negligible. 

• The ocean surface is considered flat; the curvature of the earth is neglected. 

• Course and speed inputs are taken as constants (i.e., step inputs). 

From [Refs. 5: p. 168,6: pp. 12-13], the discrete-time, state-space representation - f 
the model described above is 


A , A A 

x K+\ — < t > K x K + 10 K 


( 2 . 1 ) 


where 

jcjf+i = state estimate vector, 
x K = state vector, 

<j>x = state transition matrix and 
6) x = disturbance. 

A state vector x K is defined to contain the minimum number of the elements neces¬ 
sary to describe the target. A fourth order state vector for this model, then, consists of 
the position and velocity of the target in both x and y directions. 
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( 2 . 2 ) 


Next, a state transition matrix <f) K is chosen to fit the target dynamics. Since the target 
modeled in this problem moves linearly at a constant velocity, the <t> K matrix is 


<t>K = 


i r o o 


0 10 0 
0 0 1 T 
0 0 0 1 


where T is the observation interval. 


The unpredictable accelerations of the target are taken into account using the noise 
vector a) g . The noise vector is a function of the transition matrix T z and the acceleration 
matrix a K : 


W K ~ r K°K 


a x E 

- a yic- 


where the noise transition matrix T, is defined as 


T 2 I2 0 


r*-= 


o r 2 /2 
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Putting Equations (2.2) through (2.5) into Equation (2.1), the final state-space 
equation for the system modeled in this problem can be written as 




1 T 0 o' 

X K 


i- 

o 

l__ 


*K +1 


0 10 0 

*K 


T 0 



= 



+ 



y%+i 


bi 

O 

o 

yn 


0 T 2 I2 

-% t- 

/K+\ _ 


0 0 0 1J 

i 

A 


-1 

b, 

o 

_i 



( 2 . 6 ) 


B. THE MEASUREMENT MODEL 

For linear systems, measurements can be modeled using the following linear meas 
urement equation. [Refs. 5: p. 168,6: pp. 12-13] 


A • ,A , A 

Z K+ 1 “ 1 + t*K+l 


(2.7) 


where 

Zj r*, =» measurements, 

H ~ observation matrix, 
x K+l = state estimate vector and 
Ajr+i = measurement noise. 

Unfortunately, many real systems are not linear. The system we studied in this 
thesis falls in this category. Although this system has a linear state-transition equation, 
it has a non-linear measurement equation, since the measurements, lines of bearings, are 
non-linear functions of the system states. As it can be seen from the geometry of the 
typical scenario in Figure 1, an appropriate model with measurement noise included for 
the non-linear measurement process of this system would instead be [Ref. 3] 



5 



,N> 



Figure I. Typical Tracking Scenario 
where 


- observed lines of bearing by a sensor ship n, at time k, 

X, x » y>x ~ portion °f ^e target ship on x, y axes, at time k, 

» y* K ** position of the sensor ship n on x, y axes, at time k and 
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Ar = measurement noise. 

Although there are several types of noise which disturb the measurements, it is the 
atmospheric noise that makes the major contribution in the frequency range of interest 
in this study. This is generally a non-white, non-Gaussian process. However, it can be 
considered to be a white Gaussian process over an extended period of time in order to 
more easily implement the extended Kalman filter. In this application, a white noise 
model is used for the study of noisy cases. 
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III. THEORY 


A. KALMAN FILTER 

The Kalman filter removes random noise from the state estimates of a system by 
adding a weighted error term to the predicted state estimates. The error term is simply 
the difference between the filter's prediction of the measurement and the observed value 
of that measurement at a particular time. The weighting factor, also called the filter 
gain, is based on the predicted covariance of error between estimates and observed val¬ 
ues. The basic operation of the filter can be described in several steps: 

A priori estimates of the state x KjK are projected in time to some predicted state es¬ 
timate **„,/*, and the predicted error covariance P*„ 1/Jt of these estimates is calculated. 
The filter then calculates a gain vector G t+l , based on the predicted error covariance. 
As mentioned before, the error is the difference between observed and predicted meas¬ 
urements. Next, this error is multiplied by the filter gain and the result is added to the 
predicted state estimates to give the updated estimate ***,,*+,. The updated value of er¬ 
ror covariance / > JC+l/K+l is also calculated. 

In short, the Kalman filter is a linear, minimum variance estimator. A block dia¬ 
gram of the filter is in Figure 2. A more detailed explanation of the filter's operation 
will be given later in this chapter. For further information on the derivation and appli¬ 
cation of the Kalman filter, the reader should refer to [Refs. 7,8,9] 

B. EXTENDED KALMAN FILTER 

The Kalman filter explained above calculates the optimal estimate for the states of 
linear systems. As mentioned before, the system we studied in this thesis has a linear 
state-transition equation and non-linear measurement equation. Therefore it is not lin- 
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A Priori Information 


- Initial State Estimate 

- Initial Error Covariance 

- State Transition Matrix 

- Measurement Noise Covariance 


Observations z, 


■JC+I 


♦ t t t 


KALMAN FILTER 


State Estimate^,,**, 


Error Estimate# 


K*UK*l 


Figure 2. Block Diagram of the Kalman Filter. 


ear. The adaptation of the Kalman filter to a non-linear system is called the extended 
Kalman filter. 

For the system studied in this thesis, the non-linear measurement equation can be 
defined as: 


z *>i - H( x k+\) + t i K +1 


(3.1) 
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We see that the only difference between this equation and the linear measurement 
equation (2.7) is the observation matrix H. The H matrix is now a function of the sys¬ 
tem states. In order to linearize the measurement equation, we have chosen in this thesis 
to expand the observation matrix H in a Taylor series around the current estimate and 
then to use only the first order term. 

The following linearized form of the measurement equation is obtained directly from 
previous work on this subject. Its development will not be repeated here since an ex¬ 
cellent derivation of it is presented in these reports [Refs. 3,4]. The equations are: 


Hk+ i — 


A 

J'r, 


K+UK yn K±\ 

R~k+\ 


0 - 


Xt K±UK Xn K+i 


A <5 

R 2 


0 


AT+l 


(3.2) 


and 


«i+i-(P, 


r *+i/jr 


-y n 


JT+l 


) 2 + (*, 


M y 

r jc+i/x n Jt+i 


f 


where 

x, k*i/k ’^'jr+i/jc” The P 0 ^ 00 estimates of the target at time K+1, based on the 
previous value at time K. 

•*"*+1 » y» K+l = The position of the sensor ship n at time K+ l. 

Once the measurement process is linearized, the normal linear Kalman filter 
equations can be used in the estimation process. The following Kalman filter equations, 
taken from [Ref. 5] and derived in [Refs. 5,10], are: 


A i A 

X K+1/K ~ < P X K/K 

(3.3) 

T 

PK+l/K ** 4>Pk!K^ + Qk+ 1 

(3.4) 
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Gk+\ — ?K+ 1 / K Hk+ 1L Hk+ \PfC4-\ IK \ + P] 

(3.5) 

x K+l/K+l ~ x K+\l/C + ^K+lL z K+l ~ Hk+\ x K+\IK~] 

(3.6) 

P/c+iiK+x ~ C/- G K+l H K+l lP K+l i K 

(3.7) 


The variables are defined as follows: 

•£jc*i/ir = predicted state estimate, 

^kik~ state estimate (state vector), 

0 = state transation matrix given by equation (2.3), 

Px+ux - predicted state error covariance, 

P K/ x = state error covariance, 

Qx +l = state excitation matrix, 

G k+1 * Kalman gain matrix, 

R - measurement noise covariance matrix and 
H g+ i = linearized measurement matrix given by equation (3.2). 

The measurement noise covariance matrix R is a indication of the accuracy of the 
measurements made. This matrix is: 

P = (3*8) 

The state excitation matrix Q x ^ used in equation (3.4) represents the system noise 
process. This term is a measure of how closely the system model actually represents the 
real system and to what degree the system is affected by noise. The derivation of the 
Qk+i matrix will be studied in the next chapter. 

As can be seen from equations (3.3) through (3.7), the basic operation of the filter 
is a relatively straightforward recursive process. But the filter must be initialized before 
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processing the measurement data. When the filter is initialized, no prior value for the 
state estimate x KIK exists. Therefore the value of the first observed position is assigned 
to it. The coordinates of observed positions can be calculated from the two lines of 
bearings by using the following equations: 


- 


y 2 tan(0 2 ) +^i tan(fl,) + x 2 - jt, 
tan(0 t ) - tan(0 2 ) 


i 


y\ tan(dO + x, 


(3.9) 




y 2 tan(0 2 ) +y t tan(0,) + x 2 — x 


tan^) — tan(0 2 ) 




(3.10) 


Since there is no prior velocity information available at the moment of initialization, 
the initial velocity estimate is taken as zero. Figure 3 shows the initialization procedure. 

Since the initial state estimates will have some error, we pick some starting values 
for the errors in initial position and velocity’ to initialize the error covariance matrix. 
These are 100 nautical miles (Nm) in position and 0.5 Nm per minute (i.e., 30 kts) for 
velocity [Refs. 3,4]. The error covariance matrix can now be initialized as: 


10000 

0 



0 


0 0 0 

0.25 0 0 

0 10000 0 

0 0 0.25 


(3.11) 


Once initialized, the filter is ready to process the measurements. First, the state es¬ 
timate and state error covariance matrixes are projected to the present time using the <f> 
matrix. Next, these predicted state estimates are used to calculate the H matrix. Finally, 
the Kalman gains are calculated. The Kalman gain is a measure of where the filter's 
confidence is being placed: either in the filter's estimation or in the current observation. 
As is se in equation (3.5) the value of the Kalman gain is based on the predicted error 
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Figure 3. The Initialization Procedure. 

covariance matrix. If P K +\t * is large the Kalman gain will approach unity. If P **!/« is 
small, the gain will approach zero due to the finite value of the measurement noise 
covariance R. By manipulating equation (3.6) we can see how varying the Kalman gain 
affects the process of updating state estimates. 

*K+\IK+\ “ + Gk+1 2 K+1 (3.12) 
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As mentioned before, this equation shows that a large Kalman gain places more 
weight on the current observation. On the other hand, a small gain causes the factor 
of [/ — J in equation (3.12) to approach unity, in this sense placing more em¬ 

phasis on the filter's estimates. As can be seen from equation (3.7), the factor of 
C/— Gx+ir+t!] is used to update state error covariance matrix. 

C. SMOOTHING ALGORITHM 

Smoothing is a non-real-time data process used to reduce error in state estimates 
produced by a Kalman filter. Let time K be within the time interval 0 to N, so that 
0 <. N. A Kalman filter's state estimate for time K, denoted by x KIK , is based only 
on measurements occuring up to time K. But the smoothed state estimate is based on 
the measurements that occurred over the entire time interval 0 to N. This smoothed 
estimate is denoted by x K , y The smoothed error covariance at time K is represented by 
P KjN . This quantity has no impact on the calculation of the smoothed estimate x Klli but 
it is an indicator of how well the smoothing filter is working. If P x ,„ <, P X!X , the 
smoothed estimate is better than or equal to its filtered estimate except for the last data 
point where both smoothed and filtered estimates are equal. The smoothing algorithm 
operates backwards in time, beginning at time N and ending at time zero. Therefore, 
since the last filtered estimate at time N is taken as the first smoothed estimate, P Kjlt 
must be equal to PK/K at this last data point. This can be seen graphically in Figure 4. 

Meditch [Ref. 5] places smoothed estimates into three classes: 

Fixed-Interval smoothed estimate , denoted by x KjS where K » 0, 1,.... N-l; N is a 
positive integer. 

Fixed-Point smoothed estimate , denoted by x KIJ where J =* K + 1, K +■ 2, ....; K is a 
fixed integer. 

Fixed-Lag smoothed estimate , denoted by x KIK+K where K = 0, 1, ....; N is a fixed 
positive integer. 
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Mean square estimation error 



Figure 4. Advantage of the Performing Optimal Smoothing. 

In this thesis a Fixed-interval smoothing Filter is used. The basic block diagram oF 
this filter is shown in Figure 5. The equations to implement the smoothing algorithm 
are obtained The equations to implement the smoothing algorithm are obtained From 
(ReF. 5: pp. 216-224J and are shown below: 

“ Pkik^Pk+iik ( 3 - 13 ) 

x KI,y = ‘ X KIK + ^KL x K+\lff~ x K+IIk] (3-14) 






Figure 5. Block Diagram of the Smoothing Filter. 


Pk/n- Pkik + ^kL^k+iin~ Pk+x/kI^k (3.15) 

where 

A k = smoothing gain matrix, 
x Klff = smoothed estimate at time K, 

P K , N = smoothed error covariance at time K, 

x KtK and Pk/k = state estimate and error covariance stored by the extended Kalman 
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filter routine and 

x K + x/K and P k+U k ~ predicted state estimate and predicted error covariance stored 
by the extended Kalman filter routine. 

Several sources were helpful in understanding these equations. [Refs. 7,8,11] As it 
can be seen from equation (3.15), the smoothed estimate provided by a fixed-interval 
smoothing algorithm is simply the extended Kalman filter estimate adjusted by a 
weighted error term. The error term is the difference between the smoothed estimate 
calculated for the previous data point and the predicted estimate calculated by the ex¬ 
tended Kalman filter. It is also clear that the fixed-interval smoothing algorithm uses 
the values of x KIK and x K+l/K which are stored in the Kalman filter routine for each iter¬ 
ation. Additionally the values of P K , K and P K+ttx: must be provided for the smoothing 
routine. 
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IV. THE NOISE PROCESS IN FIXED-INTERVAL SMOOTHING 


ALGORITHM. 


A. GENERAL 

This work is devoted to studying the effects of the state excitation matrix Q K in the 
smoothing algorithm. To accomplish this, the magnitude of this matrix is changed dur¬ 
ing the assumed maneuver periods and the effects of these changes on the smoothing 
algorithm's accuracy are investigated. Also, a new maneuver detection technique is de¬ 
veloped to determine the maneuver periods. 

B. NOISE PROCESS 

The state excitation matrix Q t represents the system noise process. This matrix is 
a function of the acceleration matrix a K and the noise transition matrix T*, so that 


Qk = \_UkU T k] 


(4.1) 


where <v x is given by equation (2.4). Substituting equation (2.4) into equation (4.1), we 
find 


’*[<] 

Oyx K l E[aJJ 



(4.2) 


For reference, the noise transition matrix T* is given by equation (2.5). The Q g matrix 
allows for any random target maneuvers and also serves to account for any model in¬ 
accuracies. These inaccuracies are the differences between the true action of the target 
and its motion as characterized by equation (2.1). Q K also prevents the gain matrix G K 
from approaching zero by ensuring some uncertainty in the predicted state error 
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covariance matrix P r+i/Jf By substituting equation (2.5) into, the equation (4.2), 

equation (4.2) can be expanded as follows: 



£[<]> 2 


| £C ‘W) r ‘ 

ELa^lT 1 

T^Jr 1 £[<]r' 


The velocity of the target can be described in terms of its linear velocity and heading. 
From Figure 1 on page 6, this relationship is given as 


v * = v r sin 0, 

(4.4) 

V y - v t COS 0, 

(4.5) 


By differentiating equations (4.3) and (4.4) we obtain the target's acceleration in the 
x and y directions: 

a x — v, sin 0, + v ( 0, cos 0 f 




a x=^t ~r + 0f v y 


and 


(4.6) 


a y = v t cos 0, — v,0, sin O, 
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(4.7) 


. v y a 
a y^ v tX + v P' — 

Vy 

a y = »' f —+ 0^ 


The noise is initially described by 


££*,;] = £[0,] = O 

£[0 r ] - 4, 


(4.8) 

(4.9) 


(4.10) 


By squaring equations (4.6) and (4.7) and taking the expectations, the variances of 
target's accelerations, a, and a,, are: 


4>y=[>]H + v, J 4, 


(4.11) 


and 




(4.12) 


We also find that the covariance of a, and a, denoted by a xy or a,, is 


Elaxy^ = 




(4.13) 
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From [Ref. 12], the position of the target assuming speed is constant 


x k+\ = x k+ v x x T (4-14) 

yK+l=yK+ v yJ (4.15) 

and the position of the target assuming acceleration is constant: 

X K +1 = X K+ v Xg T + y a x J 2 (4.16) 

JV+i =yfc+ v y T + y ayj 1 (4.17) 


By comparing the equations (4.14) thorough (4.17), it can be seen that the expected 
position errors due to the unknown accelerations of the target can be defined as 

£C^ +l ]-y£C%37" (4.18) 

and 

£C7k«]--( 4.i9) 
The variances of these errors are 




(4.20) 


(4.21) 
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By comparing equations (4.20) and (4.21) with equation (4.3), we see that these 
equations are equal to elements (1,1) and (3,3) of the Q K matrix. Out of all the elements 
in the Q K matrix, these two elements have the greatest effect in compensating the posi¬ 
tion errors. Since it is most important to compensate the error on the axis which has 
the maximum error variance, the algorithm developed determines the Q K matrix using 
these elements for the magnitude of the Q z matrix. This algorithm first compares the 
error variances on the x and y axes, a, and a } , to determine which axis has the greater 
error variance. If c t, > a ,, Q K matrix becomes 

<2* = <W ( 4 - 22 ) 

where I is the unity matrix and 

If a, < a y , the Q K matrix is 

Qk ~ (4-23) 


where 


2 ( 3 . 3 ,- 


C. THE STATE EXCITATION MATRIX IN THE FIXED-INTERVAL 
SMOOTHING ALGORITHM 

As mentioned before, the fixed-interval smoothing filter uses as input the state esti¬ 
mates and error covariances calculated by the forward-time Kalman filter. But in order 
to see the effects of the state excitation matrix Q K in the smoothing algorithm, the pre¬ 
dicted error covariance matrix P K+UK is recalculated in the smoothing routine. The pre- 
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dieted state estimates x K+llK are also recalculated in the smoothing routine. By 
recalculating these matrices, we attempted to get a feeling for the expected magnitude 
of the smoothing error. The intent was to enable the the filter to carry along its own 
error analysis. The new system of the recursive equations for the fixed-interval 
smoothing becomes: [Refs. 4,13] 


A , A 

X K+\IK ~ <P X KIK 

(4.24) 

T 

P K+\/K — $PkIK$ + Qk 

(4.25) 

A K ~ Pk/K4 T PK+\IK 

(4.26) 

X KIN ~ X K/K + A kL x K+\/N ~ ^AM-t/Af] 

(4.27) 

P KIN ~ PkIK + A k[Pk+\IN ~ Pk+\IK^ A K 

(4.28) 


As seen from equation (4.26), the smoothing filter gains are a function of the error 
covariance. As the predicted error increases, the smoothing gains decrease due to the 
inverse relationship between smoothing gains and the predicted error covariance matrix. 
In this way the smoothing filter can compensate for a large expected error by placing 
more emphasis on the Kalman filter estimates. By substituting equation (4.24) into 
equation (4.27), we obtain 

X K T//V - X KIK +/4 tf[- x Ar+l/V “ 4> x kik1 

x KjN = £1 ~ A K$ 1 x K/K + A K x K+\/N (4.28) 

Equation (4.28) shows that a small A e causes the factor of £1 — A to approach 
unity, thereby placing more emphasis on the forward-time Kalman filter estimates x KjK . 
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We can exploit this behavior of the smoothing filter and use it to adapt the smoothing 
filter to detected target maneuvers. 

D. MANEUVER DETECTION 

Should the target maneuver during the tracking process, the filtered estimates tend 
to diverge from the true estimates. This introduces error into the state estimates. 
Therefore a procedure must be developed to detect the target's maneuvers. This can be 
accomplished by monitoring the filter residual process. 

The residual process of the extended Kalman filter is taken as the difference between 
the observed position and the filter's predicted position estimates. This process can be 
defined as 


Z K - X K/K -11 


(4.29) 


The maneuver detection technique implemented calculates the residual value for each 
observation and compares this to the two maneuver gates. The gates are defined as 
three times and eight times the predicted standard deviation. Some of the principles 
underlying this technique are presented in [Ref. 14]. To define the predicted standard 
deviation, error ellipse equations are used. More detailed information about error el¬ 
lipses can be found in [Ref. 6: pp. 17-18]. These equations are: 


2 °x + a y cov(xy) 
<V_ 2 + sin 2d 


(4.30) 


2 + °y cov{xy) 

2 sin 2d 


(4.31) 


where 

o\ and <tJ = variances in the original cartesian coordinate system, 
a\. and o 2 y - variances along the major and minor axis oriented by 
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2cov(jg;) 



(4.32) 


By taking the square roots of equations (4.30) and (4.31), the standard deviations 
on the x' and y' axes of the error ellipse are 


<Xv- = 


a \ + cov(xy) 


sin 26 


(4.33) 


and 


4 


2 2 

°x + a y cov{xy) 
2 sin 26 


(4.34) 


The maneuver detection algorithm compares the two standard deviations which are 
represented by the lengths of the x' and y' axes of the error ellipse shown in Figure 6. 
It selects the larger one as a predicted standard deviation, allowing the gates to take on 
the following values: 


LO WER_GA TE = 3<t* 


and 


UPPERJ3ATE- 8<7* 

where a K is the larger of the two standard deviations, a x . or a r . 

The reason for choosing the value of 2a K for the lower gate is well explained in [Ref. 
14], The value of the upper gate, known as a "Glitch" gate, is dependent on the opera¬ 
tional characteristics of the target. This gate rejects motions that the target could not 
possibly make. In our problem, extremely high linear or tangential accelerations are 
examples of such behavior. When the residual exceeds this gate, the filter recognizes 
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Figure 6. Error Ellipse 


that this motion is impossible for the given target and so must be due to noise. The 
value of 8 <t* gave the best results in this application. 

For each observation, the calculated residual is compared to the two gates by the 
maneuver detection algorithm in the extended Kalman filter routine. If the residual is 







less than the value of the lower gate, the filter continues on and processes the next ob¬ 
servation. If the residual is larger than the value of the upper gate, the filter ignores that 
observation by setting filter gains equal to zero, thereby making the state estimates equal 
to the predicted estimates, 

x K+\IK+l “ X K+\!K (4-35) 

This procedure will work well for isolated bad observations. However, if there are se¬ 
veral consecutive bad observations, the filter can conceivably lose track of the target as 
the filter's state estimates diverge more and more away from the actual target states. 
To remedy this, the extended Kalman filter sets the filter gains equal to zero only for the 
first of two consecutive bad observations but uses non zero gain for the second. If the 
residuals of the two consecutive observations are in the zone between the two maneuver 
gates, shown as concentric circles in Figure 7, a maneuver is detected and compensation 
algorithm begins. The value of two provides a trade-oIT between fast response and low 
false alarm rates. 

The maneuver detection algorithm does not run a second time in the fixed-interval 
smoothing routine, since it can use the maneuver times detected in the extended Kalman 
filter with no loss of accuracy. Additionally, the fixed-interval smoothing algorithm 
"backs up" and considers the first point ignored by the Kalman filter as a maneuver 
point, since it knows that if the maneuver is detected at some observation time in the 
Kalman filter routine, it must have started one observation earlier. 

During the compensation, the state excitation matrix Q K is increased by multiplying 
the coefficients along the main diagonal by a factor of 2.0. These coefficients account 
for random course and speed changes of the target. As the Q K matrix is increased, the 
predicted error covariance PK+lIK is also increased because of the direct effect of the Q K 
matrix on th nagnitude of the predicted error covariance. And, as the P K + UK matrix 
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♦ PREDICTED ESTIMATE 

• PREVIOUS OBSERVATION 
O CURRENT OBSERVATION 

A. NO MANEUVER 

B. MANEUVER 

C. BAD OBSERVATION 


Figure 7. Diagramming the Maneuver Detection Technique 

increases, the outputs of both the extended Kalman filter and the smoothing filter are 
affected as explained before. The multiplicative constant of 2.0 was found by trial and 





V. COMPUTER SIMULATIONS 


A. GENERAL 

The SHIPTRACK.FOR extended Kalman filter algorithm was first implemented in 
[Ref. 3] on an Apple Macintosh Plus microcomputer. In [Ref. 4] the fixed-interval 
smoothing algorithm was added, the new algorithm was named SHIPSM.FOR this al¬ 
gorithm was adapted to run on an IBM PC. This research takes the program one step 
further by adding new algorithms to detect maneuvers and to derive the state excitation 
matrix Q x . A new program SHIPMANE is used in the following manner for the simu¬ 
lations. 

The raw data required by the SHIPMANE.FOR is generated by RAWDATA.FOR. 
This program is modified from the program TRACK.FOR used in [Ref. 3]. Our inten¬ 
tion was to make the target follow a circular track during the maneuver period rather 
than make a sharp turn. Program RAWDATA.FOR asks the user for the initial posi¬ 
tions, speeds and courses of the target and the tracking ships, the total tracking time and 
the observation interval. It also requests the desired maneuver period and any speed and 
course changes of the target during this period. The outputs consist of noisy or noise 
free bearings from each tracking ship to the target, the updated positions of all the ves¬ 
sels and the time of the observation are stored in the file called TRKDATA.DAT. 

The program SHIPMANE.FOR reads and processes the data stored in 
TRKDATA.DAT. The outputs of this program are mainly stored in three files. The 
first file FILDATA.DAT stores the results of the extended Kalman filter portion of the 
program SHIPMANE.FOR while the fixed-interval smoothing results are included in 
the second file SMDATA.DAT. The results of the maneuver detection algorithm are 
stored in the third file MANEUDATA.DAT during the process of the extended Kalman 
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filter portion of the SHIPMANE.FOR. Additionally, a fourth file TRUDATA .DAT, 
is created for graphic purposes, and consists of the actual positions (tracks) of the target. 
Although this file is useful for the purposes of this thesis, in real world tracking problem 
this information would seldom, if ever, be available. In this thesis the terms "real" or 
"actual", when applied to tracks or maneuvers, refer to the data contained in this file. 
"Assumed" tracks or maneuvers refer to what is detected by the extended Kalman filter 
or the smoothing routine. 

The MATLAB graphic routines are used to obtain the graphical representations of 
the data included in the output files of the SHIPMANE.FOR. Five graphic outputs are 
obtained for each simulation case except for the third case, which has only two. For all 
cases except the third, the first graph is a geographic plot which show extended Kalman 
filtered track versus the the actual and observed target tracks. The second graph com¬ 
pares the track resulting from the fixed-interval smoothing with the actual and observed 
target tracks. The third graph is the time plot shotting the filtered, smoothed and ob¬ 
served position errors. The fourth is also a time plot and shows the residuals for each 
observation along with the threshold values of the upper and lower maneuver detection 
gates. In the third simulation case, only this graph is included. The fifth graph gives the 
overall results for each case. Due to the limited number of variables which can be used 
in the single MATLAB graphic package, the true track of the target was shown as a line 
without each observation point being shown. 

Although both of the programs SHIPMANE.FOR and RAWDATA.FOR can eas¬ 
ily be modified for multi-bearing measurements, the simulation cases used only two 
bearings per observation as measurements, one from each tracking ship. The set of the 

simulations studied in this chapter consists of following cases: 

• Case #1: 60° maneuver toward tracking ships, with noiseless measurements. 

• Case #2: 60° maneuver away from tracking ships, with noiseless measurements. 
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• Case #3: Test for the maneuver detection algorithm. (1) Case #1 with different 
maneuver period. (2) Case #2 with different maneuver period. 

• Case #4: 60° maneuver toward tracking ships, with noisy measurements. 

• Case #5: 120° maneuver toward tracking ships, with noisy measurements. 

• Case #6: 60°maneuver away from tracking ships, with noisy measurements. 

• Case #7: 120° maneuver away from tracking ships, with noisy measurements. 

In all cases, the target ship starts at the position (-75,150). The initial course of the 
target is 090° and the initial speed of the target is 15 knots for each case. The speed of 
the target is held constant throughout the simulation cases. The initial positions of the 
tracking ships are (-40,0) and (-60,0), and courses and speeds are 030° and 10 knots for 
each case. The speeds and courses of the tracking ships are also held constant. The 
observation period is 30 minutes and all cases run for 450 minutes. 

The success of the algorithm can be expressed by the percentage improvement be¬ 
tween the total error in observed positions and the total errors in the filtered estimates 
and smoothed estimates, respectively. This percentage indicates of how much the ex¬ 
tended Kalman filtering and the fixed-interval smoothing improve the position accuracy 
over the observations. For the extended Kalman filter, this percentage is simply the ra¬ 
tio between the the total error in the observed positions and the total error in the filtered 
estimates throughout the simulation case or time period of interest. In some cases this 
was recalculated specifically for maneuver periods. The percentage improvement due to 
the smoothing was similarly the ratio between the total error in the observed position 
estimates and the total error in the smoothed position estimates. Also, the average po¬ 
sition errors due to the extended Kalman filter and the fixed-interval smoothing algo¬ 
rithm are given for the different cases. The average position error due to the extended 
Kalman filter is calculated by summing the position errors of the filtered position esti¬ 
mates and then dividing by the total number of observations. The average position error 
due to the smoothing routine is also found by summing the position errors of smoothed 
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estimates over the entire simulation (or in some cases, over the number of observations 
of interest) and dividing by the number of observations. The average position errors 
show how well the extended Kalman filter and fixed-interval smoothing algorithm work 
for a particular simulation case. 
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B. CASE #1 


The target is steaming due east at 15 knots at the beginning of this case. Between 
time equals 150 minutes and and time equals 300 minutes, it makes a 60° course change 
toward the advancing tracking ships on a circular track. The results for the filtering and 
smoothing are shown in Figure 8 and Figure 9. Since there is no noise in the meas¬ 
urements, the observed track equals the true track. Although the measurements are not 
noisy, the filter estimates diverge very slightly for the first several observations. This 
initial error, shown in Figure 10, is due to the inaccuracy of the initial state estimates. 
This inaccuracy also causes the high values for the upper and lower maneuver gates for 
the first few observations, as can be seen in Figure 11. When the target starts its turn 
at time equals 150, the tracking error begins to increase. It decreases, however, as the 
filter regains the target track and it reaches zero one observation after the target finishes 
its maneuver. 

The fixed-interval smoothing algorithm improves the position accuracy over the ex¬ 
tended Kalman filter by an average of 35% during the real maneuver period, between 
time equals 150 and equals 300 minutes, and 22% during the overall simulation. 

As can be seen in Figure 11, the residuals appear between the upper and lower 
maneuver gates for time equals 240,270 and 300. Since two consecutive residuals be¬ 
tween the upper and lower gate values are necessary for the maneuver to be detected, 
the extended Kalman filter recognizes times 270 and 300 as a maneuver period. Time 
240 is ignored by the Kalman filter. The smoothing algorithm, however, does not ignore 
time 240, since it knows that if the maneuver was detected at time 270, it must have 
begun at time 240. Therefore, the maneuver period for the fixed-interval smoothing al¬ 
gorithm is taken as times 240, 270 and 300. The overall results of this case can be seen 
in Figure 12. 
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Figure 10. The Position Errors for Case ill 
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Figure 11. The Results of the Maneuver Detection Algorithm for Case #1 
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Figure 12. The Overall Results for Case # 1 
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C. CASE #2 


In this case, the target makes a 60° maneuver away from the two tracking ships. 
The target's initial course is 090° at 15 knots. Between times 150 and 300, it turns 
northeast to a new course , 030°, on a circular track. Again, the observed and true 
tracks are the same due to the lack of measurement noise. T results of filtering are in 
Figure 13 and the results of the smoothing routine are in Figure 14. The initial error 
in Figure 15 and the high maneuver gate values for the first few observations in 
Figure 16 are again due to the error in the initial estimates. 

The filter error starts to increase when the target begins to maneuver at time 150. 
When the target completes its maneuver, the error approaches zero. Since the target 
starts to pull away from the tracking ships, the filter error reaches zero later than it did 
in the previous case. From Figure 15, we can see how the fixed-interval smoothing 
routine improves the filter's estimate. The smoothing algorithm decreases the position 
error of the extended Kalman filter by an average of 22% for the overall case and by an 
average of 38% for the real maneuver period between 150 and 300 minutes. 

From Figure 16, the residuals at times 240, 270 and 300 are in the maneuver zone. 
The maneuver period is detected for times 270 and 300 for the extended Kalman filter 
and for times 240, 270 and 300 for the smoothing filter. The final results of this case are 
in Figure 17. 
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Figure 14. The Results of the Fixed-Interval Smoothing for Case #2 
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Figure 15. The Position Errors for Case #2 
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Figure 16. The Results of the Maneuver Detection Algorithm for Case # I 
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Figure 17. The Overall Results for Case #2 
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D. CASE #3 


In this case, the two previous cases are tried with different maneuver periods in order 
to test the performance of the maneuver detection algorithm. Therefore only the figures 
which show the results of the maneuver detection algorithm are included. 

1. Case #1 With Different Maneuver Period 

In this part of the case, Case #1 is again tried with the new maneuver period 
from time 270 to time 390. From Figure IB, it can be seen that the residuals at times 
300, 330, 360 and 390 are between the upper and lower gates. The maneuver period is 
between 330 and 390 minutes for the extended Kalman filter algorithm and between 
times 300 and 390 for the fixed-interval smoothing algorithm. 

2. Case #2 With Different Maneuver Period 

In this part, Case #2 with a new maneuver period, this time between 90 and 270 
minutes, is simulated. In Figure 19, the residuals are in the maneuver detection zone 
at 180, 210, 240 and 270 minutes. In the extended Kalman filter routine, the maneuver 
detection algorithm detects the maneuver at 210, 240 and 270 minutes. For the fixed- 
interval smoothing algorithm, the maneuver period begins at time 180 and ends after 
time 270. 
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Figure 19. Tlie Results of the Maneuver Detection Algorithm for Case #3-(2) 








E. CASE #4 


This case is the same as Case #1 except noise is added to the measurements. The 
filtering and smoothing results for this case are shown in Figure 20 and Figure 21. At 
the beginning of the tracking problem, the error is high. However, after the target ma¬ 
neuvers, the vessels close each other and the position error decreases rapidly. 

From Figure 23, it is seen that the residuals are between the maneuver gates at 
times 180 through 330. The maneuver period for the extended Kalman filter is from 210 
to 330 minutes. During this period the improvement in position error due to the Kalman 
filter is 32%. The maneuver period for the smoothing filter is between times 180 and 
330, and the position error improvement due to the smoothing filter is 78%. 

It is also seen that the maneuver detection algorithm recognizes a bad observation 
at time 60. As Figure 20 shows, the extended Kalman filter estimates for this point 
appear to be only the projections of the previous estimates in time. Therefore the posi¬ 
tion error of the extended Kalman filter for this point is 145% worse than the observed 
position error, while the smoothed position error is only 4% worse. The average posi¬ 
tion error is 3.8 Nm for the extended Kalman filter and 2.5 Nm for the smoothing filter 
over the entire tracking period. The overall results for this case are in Figure 24. 
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Figure 22. The Position Errors for Case #4 
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Figure 23. The Results of the Maneuver Detection Algorithm for Case #4 
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F. CASE #5 


This case includes a 120° turn which, if undetected, will cause an unacceptably high 
error. The target turns to a new course of 210° by following a circular track between 
times 150 and 300. Again, the observed position error decreases rapidly after the target 
completes its maneuver, since all the vessels start to close each other. The extended 
Kalman-filtered and smoothed results can be seen in Figure 25 and Figure 26. 

The observed, filtered and smoothed position errors are shown in Figure 27. The 
extended Kalman filter improves the position accuracy by 46% over the observed posi¬ 
tion errors for the entire tracking period, and the improvement due to smoothing is 73% 
over the same period. The maximum filtered position error is around 6 Nm except at 
time zero, while the average filtered error is 3 Nm. The maximum smoothed position 
error is 4 Nm and the average smoothed position error is 1.5 Nm. 

The maneuver period for the extended Kalman filter is from 180 through 330 min¬ 
utes, during which the improvement due to the extended Kalman filter is 46%. The 
maneuver period for the fixed-interval smoothing is between times 150 and 330, and the 
position accuracy is 55% over the observed position errors for the maneuver period 
alone. The observation at time equals 120 is recognized as a bad observation. The 
overall results are in Figure 29. 
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Figure 25. Hie Results of the Kalman Filter Tracking for Case #5 
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Figure 26. Tlie Results of the Fixed-Interval Smoothing for Case #5 
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Figure 21 . The Position Errors for Case #5 
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Figure 28. The Results of the Maneuver Detection Algorithm for Case #5 
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G. CASE #6 


This case is the same as Case #2, with the addition of noise to the measurements. 
The results of the extended Kalman filtering and smoothing are in Figure 30 and 
Figure 31. 

From Figure 32, the maximum Kalman filter position error is 8 Nm at time equals 
30 while the average filtered position is 3.3 Nm. The maximum errors are 3.5 Nm at 
time 420 and 6.5 Nm at time 450, which the filtered and smoothed errors had to be same, 
and the average error is 2.1 Nm for the smoothed errors. This case shows the general 
improvement in the filtered and smoothed estimates. The position accuracy increases 
by 47% with the extended Kalman filter and by 65% with the fixed-interval smoothing 
filter. 

As seen in Figure 33, the residual values are in the maneuver zone at times 60, 180, 
210, 240, 270 and 330 minutes. No maneuver detection occurs at times 60 and 330, since 
the residuals immediately following these times are out of the maneuver zone. The ma¬ 
neuver periods are detected from 210 to 270 minutes for the extended Kalman filter and 
from 180 to 270 minutes for the smoothing filter. The improvement in the accuracy of 
the position estimates is 49% due to the extended Kalman filter and 60% due to the 
smoothing filter. 

Figure 33 also shows that the maneuver detection algorithm recognizes the obser¬ 
vations at times 120, 300 and 420 as bad observations. As can be seen from Figure 30, 
the filtered positions are the projections of the previous estimates in time with no noise 
adaptation being made. For each of these times the filtered estimates are more accurate 
than the observed estimates and the smoothed estimates are the most accurate of all. 
The average improvement in the position estimate for these three observations is 56% 
for the extended Kalman filter and 83% for the smoothing filter. Figure 34 shows the 
overall tracking and smoothing results for this case. 
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Figure 32. The Position Errors for Case #6 
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Figure 33. The Results of the Maneuver Detection Algorithm for Case #6 
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Figure 34. The Overall Results for Case #6 
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H. CASE #7 


This case depicts a 120 8 target maneuver away from the tracking ships. The filtered 
and smoothed tracks are in Figure 35 and Figure 36. 

The observed, filtered and smoothed position errors are in Figure 37. The accuracy 
of the position estimates is increased by 47% with the extended Kalman filter and by 
65% with the fixed-interval smoothing throughout the entire tracking period. The av¬ 
erage position error due to the Kalman filter is 5.4 Nm while the average position error 
of the smoothed estimates is 2.3 Nm. The smoothed error is always less than 5 Nm with 
the exception of the last observation time (i.e. time 450) where the error is 5.4 Nm. 

From Figure 38, the maneuver period is detected as times 180, 210, 240 and 270 for 
the extended Kalman filter and as times 150, 180, 210, 240 and 270 for the smoothing 
algorithm. During these periods, the accuracy in the position estimates is improved by 
42% with the Kalman filter and by 72% with the smoothing. The observations of times 
90 and 300 are recognized as bad observations. The Kalman filter improves the position 
accuracy by an average of 68% and the average improvement due to the smoothing 
routine is 90% for these two points. The overall results for this case are shown in 
Figure 39. 
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Figure 37. The Position Errors for Case #7 
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Figure 38. The Results of the Maneuver Detection Algorithm for Case #7 
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Figure 39. The Overall Results for Case #7 





VI. CONCLUSIONS 


We have tried to improve the accuracy of the extended Kalman filter with a fixed- 
interval smoothing routine by implementing a new maneuver detection algorithm. 
Whereas maneuver detection algorithms are normally applied only to the extended 
Kalman filter, we apply this algorithm both to the extended Kalman filter and to the 
fixed-interval routine to adapt them both to unpredicted maneuvers of the target. We 
studied the effects of varying the state excitation matrix Q K in the fixed-interval 
smoothing during the assumed maneuver periods. Several simulation cases were run and 
analyzed in order to test the performance of the algorithm. 

Although some maneuver points were missed, the maneuver detection algorithm 
worked well during the simulations. The probabilities of a maneuver being detected for 
noise free and noisy cases are shown in Table 1 and Table 2. In order to obtain these 
probabilities a large number of simulations {i.e. 10) for both noise free and noisy cases 
were run on the IBM PC. Due to space constraints, just four representative runs each 
were presented in the previous chapter.. However, in Table I and Table 2 the results 
of all ten runs are shown for the fixed-interval smoothing routine only. To get the 
probabilities of a maneuver being detected in the extended Kalman filter, the reader must 
shift the numbers in both tables one cell right. In both Table 1 and Table 2, the ma¬ 
neuver was executed at N equal zero. 

With a new maneuver detection technique, the fixed-interval smoothing routine im¬ 
proved the accuracy of the target's position estimates in all the simulation cases. This 
improvement was 35-/5% over the observed target positions and over 35-55% over the 
Kalman filter's estimates. Applying the new maneuver detection technique also im¬ 
proved the accuracy of the extended Kalman filter by 45-50% over the entire time in- 
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Table 1. Probabilities of a Maneuver Being Detected at Point N in a Noise Free 
Enviroinment for Fixed-Interval Smoothing Algorithm (Maneuver Executed 
at N = 0) 


El 

N= 1 

N = 2 

N= 3 

N = 4 

N= 5 

0% 

0% 

25% 

75% 

100% 

100% 


Table 2. Probabilities of a Maneuver Being Detected at Point N in a Noisy 
Enviroinment for a Fixed-Interval Smoothing Algorithm (Maneuver Exe¬ 
cuted at N = 0) 


N = 0 

N= 1 

N = 2 

II 

Z 

20% 

80% 

100% 

100% 


terval. Where the accuracy was most improved by this technique during the maneuver 
periods: here the accuracy increased by 30-60% using the extended Kalman Filter and 
60-80% using the the fixed-interval smoothing algorithm over the observed positions 
during the actual maneuver periods. And during the maneuver periods the smoothed 
estimates were 30-70% more accurate than the Kalman Filter's estimates. 

These significant improvements were obtained, in pan, by using the time-varying 
values of the state excitation matrix Q K . However, there is a disadvantage to this tech¬ 
nique. Since this matrix is added to the predicted error covariance matrix P K , t . u high 
values of the matrix Q K will cause the predicted error covariance matrix to grow 
boundlessly which will make the filter become unstable. Also, increasing the magnitude 
of the state excitation matrix in the fixed-interval smoothing algorithm makes the 
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smoothing filter estimates diverge to the extended Kalman filter estimates. This in¬ 
creases the need for a more accurate extended Kalman filter, since the accuracy of the 
smoothed estimates in this case depends to a large degree on the extended Kalman fil¬ 
ter's estimates. 

There are at least two areas which can be investigated to develop the tracking algo¬ 
rithm more fully. The first is research in new noise models. The model used was a white 
noise process. Although this model is relatively adequate for representing atmospheric 
noise over an extended time period, better models could be used which take into account 
random noise spikes, the lightning effects, of the atmospheric noise process. The second 
area is adapting the algorithm for multi-target tracking. Improving the ability of the 
algorithm to track and identify two or more targets would have great value in ship 
tracking and targeting problems. 








APPENDIX A. THE EXTENDED KALMAN FILTER WITH FIXED 
INTERVAL SMOOTHING ALGORITHM 

C *** SHIPMANE.FOR *** 

Qi r ki f k k A A A A"A k k ir kMrk-krkMf iri cifkirk i rirk-kiflt -k k-kk A ' A A" A' A A jHHr frl Hnfc A A A A ' A -kit h ' A A A A A A ' A"A"A i A 

C* * 

C* THIS A EXTENDED KALMAN FILTER TRACKING ROUTINE WITH THE FIXED * 

C* INTERVAL SMOOTHING ALGORITHM. THIS PROGRAM USES BEARINGS TAKEN * 

C* FROM TWO SENSOR SHIPS TO THE TARGET. A NEW MANEUVER DETECTION * 

C* ROUTINE IS IMPLEMENTED. THE NEW ALGORITHM TO DERIVE THE STATE * 

C* EXCITATION MATRIX Q IS ALSO DEVELOPED. TO RUN THE PROGRAM: * 

C* * 

C* 1) RUN THE PROGRAM <RAWDATA. FOR> LOCATED IN APPENDIX B TO * 

C* PRODUCE THE RAW DATA. * 

C* * 

C* 2) RUN THE <SHIPMANE.FOR> * 

C* * 

C* THE OUTPUTS OF THE PROGRAM STORED IN THE FOLLOWING FILES: * 

C* * 

C* 1) THRDATA = INCLUDES THE THERESHOLD VALUES OF THE * 

C* MANEUVER GATES AND RESIDUAL CALCULATED * 

C* FOR EACH OBSERVATION. * 

C* * 

C* 2) CIRCDATA = INCLUDES THE REQUIRED DATA TO DRAW THE * 

C* MANEUVER GATES AS A CONCENTRIC CIRCLES * 

C* AROUND THE PREDICTED FILTER ESTIMATES. * 

C* * 

C* 3) BEGINDATA = INCLUDES THE FIRST POINTS, IGNORED BY * 

C* THE EXTENDED KALMAN FILTER, OF THE TARGET * 

C* MANEUVERS TO BE USED BY THE FIXED-INTERVAL * 

C* SMOOTHING ALGORITHM. * 

C* * 

C* 4) MANEUDATA = INCLUDES THE DETECTED MANEUVER POINTS. * 

C* * 

C* 5) TRUDATA = INCLUDES THE ACTUAL POSITION OF THE TARGET * 

C* FOR EACH OBSERVATION TIME. * 

C* * 

C* 6) FILDATA = INCLUDES THE EXTENDED KALMAN FILTER'S POSITION * 

C* ESTIMATES ALONG WITH THE OBSERVED POSITIONS, * 

C* KALMAN FILTER ERROR AND OBSERVATION ERROR. * 

C* * 

C* 7) SMDATA = INCLUDES THE SMOOTHED POSITION ESTIMATES AND * 

C* SMOOTHING ERROR. * 

C* * 

C* TO GET THE GRAPHIC RESULTS: * 

C* * 

C* 1) COPY THE FILES TRUDATA, FILDATA, SMDATA AND MANEUDATA INTO * 

C* THE MATLAB SUB-DIR. * 

C* * 

C* 2) RUN THE PROGRAM <SHIPTR. M> IN THE MATLAB SUB-DIR. THE * 

C* GRAPHIC RESULTS WILL BE STORED IN THE META FILE SHIPTR. MET. * 

Qi r /rkiti f f r k’fck kit A k kir k-ki c f r k lt ' k - . ' r kititit i'c. ' n' i k A ' A - klrk k- k A k ic trlr k-/r lr i r > r tr \ k k A A ir/rrk k A ' l V i V A'-A A k k ' k - / rk' .\ " k 
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C ***VARIABLE DEFINITIONS*** 


AK 

AKT 

BD 

BOC 


BRG 

BRKKM1 

DBRG 

DT 

DTOR 

FAC1 

G 

H 

HDG 

HT 

I 

IMAT 

J 

K 

PDIFF 


PHI 

PHIT 

PKK 

PKKS 

PKKM1 

PKKM1S 

IPKKM1S 

PSS 

PX 


PY 


Q 

R 

RANGE 

RTOD 

SHDG 

SPD 

SPKKM1 

SSPD 

SXPOS 

SYPOS 

TEMP 


SMOOTHING FILTER GAIN MATRIX 
TRANSPOSE OF AK 
BAD OBSERVATION INDICATOR 
BAD OBSERVATION COUNTER WHICH PROVIDES 
THAT ONLY THE FIRST OF TWO CONCECUTIVE 
BAD OBSERVATIONS WILL BE RECOGNIZED 
MEASURED TARGET BEARING IN RADIANS 
PREDICTED TARGET BEARING MEASUREMENT 
IN RADIANS, BRG(K/K-1) 

MEASURED TARGET BEARING IN DEGREES 
TIME DELAY BETWEEN OBSERVATIONS, 

T(K) - T(K1) 

DEGREE TO RADIAN CONVERSION FACTOR 
RECIPROCAL OF VARE 
KALMAN GAIN VECTOR 
MEASUREMENT MATRIX 

TARGET HEADING IN DEGREES BY KALMAN FILTER 

TRANSPOSE OF H 

COUNTER 

4X4 IDENTITY MATRIX 
COUNTER 

ITERATION INTERVAL 

POSITION DIFFERENCE BETWEEN OBSERVED AND 
PREDICTED STATE ESTIMATES 

| Z(K) - XCK/K-l) | 

DISCRETE-TIME STATE TRANSITION MATRIX 
TRANSPOSE OF PHI 

ESTIMATION ERROR COVARIANCE MATRIX, P(K/K) 
SMOOTHED ERROR COVARIANCE MATRIX 
PREDICTED ESTIMATION ERROR COVARIANCE 
MATRIX, PCK+l/K) 

PREDICTED ERROR COVARIANCE MATRIX FOR 
SMOOTHING, P(K+1/K) 

INVERSE OF PKKM1S 

ERROR COVARIANCE MATRIX FOR SMOOTHING, P(K/K) 
POSITION DIFFERENCE IN X DIRECTION BETWEEN 
OBSERVED AND PREDICTED STATE ESTIMATES 
| ZX - X(K/K-l)(1,1) | 

POSITION DIFFERENCE IN Y DIRECTION BETWEEN 
OBSERVED AND PREDICTED STATE ESTIMATES 
| ZY - X(K/K-1)(3,1) | 

STATE EXCITATION MATRIX 
MEASUREMENT NOISE COVARIANCE 
DISTANCE FROM SENSOR TO A PRIORI TARGET 
POSITION 

RADIAN TO DEGREE CONVERSION FACTOR 
TARGET HEADING IN DEGREES BY SMOOTHING 
TARGET SPEED IN KNOTS BY KALMAN FILTER 
STORE THE INITIAL ERROR COVARIANCE, P(0/-1) 
TARGET SPEED IN KNOTS BY SMOOTHING 
SMOOTHED TARGET POSITION IN X DIRECTION 
SMOOTHED TARGET POSITION IN Y DIRECTION 
TEMPORARY STORAGE MATRICES USED IN MATRIX 
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TIMEX 


TIMEXLO 

TL 

TLS 

TU 

TUS 

VARE 

XDIFF 

XKK 

XKKS 

XKKM1 

XKKM1S 

XPOS 

XS 

XSS 

XT 

YDIFF 

YPOS 

YS 

YT 

ZX 

ZY 


OPERATIONS 

IMPOSSIBLY HIGH CONSTANT FOR DETERMINING THE 
FIRST POINT OF THE MANEUVER WHICH IS IGNORED 
BY THE KALMAN FILTER AND ACCOUNT BY THE SMOOTHING 
FILTER 

VARIABLE FOR STORING THE MANEUVER POINTS 
THRESHOLD VALUE FOR THE LOWER MANEUVER GATE 
VECTOR VARIABLE FOR STORING THE THRESHOLD 
VALUES OF THE LOWER MANEUVER GATE 
THRESHOLD VALUE FOR TOE UPPER MANEUVER GATE 
VECTOR VARIABLE FOR STORING THE THRESHOLD 
VALUES OF THE UPPER MANEUVER GATE 
VARIANCE OF RESIDUALS PROCESS 
DISTANCE IN X DIRECTION FROM SENSOR TO A 
PRIORI TARGET POSITION 
ESTIMATED TARGET STATE VECTOR, X(K/K) 

SMOOTHED TARGET STATE VECTOR 
PREDICTED TARGET STATE VECTOR, X(K/K-1) 

PREDICTED TARGET STATE VECTOR FOR SMOOTHING, X(K+1/K 
KALMAN FILTERED TARGET POSITION IN X DIRECTION 
SENSOR POSITION IN X DIRECTION 
TARGET STATE VECTOR FOR SMOOTHING, X(K/K) 

TRUE TARGET POSITION IN X DIRECTION 
DISTANCE IN Y DIRECTION FROM SENSOR TO A 
PTIORI POSITION 

KALMAN FILTERED TARGET POSITION IN DIRECTION 
SENSOR POSITION IN Y DIRECTION 
TRUE TARGET POSITION IN Y DIRECTION 
OBSERVED POSITION IN X DIRECTION 
OBSERVED POSITION IN Y DIRECTION 


C *** VARIABLE DECLARATIONS *** 


REAL*4 XKK(4,1),XKKM1(4,1),PHI(4,4),SXPOS,SYPOS,HDG,PDIFF 
REAL*4 H(1,4),G(4,1),TEMP1(1,4),TEMP2(1,1),TEMP3(4,1),ZT 
REAL*4 TEMP4(4,4),TEMP5(4,4),PKK(4,4),PKKM1(4,4),HT(4,1) 
REAL*4 LXKK(4,1),LPKK(4,4),XS(10),YS(10),DBRG(10),BRG(10) 
REAL*4 TEMP6(4,4),PHIT(4,4),IMAT(4,4),XT,YT,SHDG,XPL(100) 
REAL*4 VARE(2),E(2),G11,G13,G21,G23,Q(4,4),SSPD(100),MC 
REAL*4 DT,XDIFF,YDIFF,RANGE,XS1,YS1,BRG1,BRKKM1,YPL(100) 
REAL*4 OBSERR(200),FAC1,SIGTHT,SIGVT,R,RTOD,SPD(100),BD 
REAL*4 XS2,YS2,BRG2,ZX,ZY,DTOR,TRKERR(100),TL,TU,SX,SY 
REAL*4 XNNM1(4,1),XSS(4,1),XKKM1S(4,1),THETA,PY,PX,PD 
REAL*4 PNNM1(4,4),PSS(4,4),PKKM1S(4,4,100),IPKKM1S(4,4) 
REAL*4 AK(4,4),AKT(4,4),STRKERRC100),DTS(100),SPKKM1(4,4) 

REAL*4 TEMP1S(4,4),TEMP2S(4,1),TEMP3S(4,1),TH1(4,1),YPOS 
REAL*4 TEMP4S(4,4),TEMP5S(4,4),TEMP6S(4,4),TH2(4,4),XPOS 
REAL*4 XKKS(4,1,100),PKKS(4,4,100),TLS(100),TUS(100),DR(100) 
REAL*4 XPU(100),YPU(100),BOC 

INTEGER*4 TIME,TIMEP(100),NP,TIMEX,TIMEXB(100),TIMEXLO(100) 

C *** OPEN OUTPUT DATA FILES *** 

OPEN(UNIT=2,FILE='TRKDATA. DAT',STATUS='OLD') 
OPEN(UNIT=3,FILE='THRDATA. DAT 1 ,STATUS=’NEW') 

OPEN(UNIT=4,FILE='CIRCDATA. DAT',STATUS=’NEW') 
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OPEN(UNIT=6,FILE='MANEUDATA. DAT',STATUS='NEW') 

OPEN(UNIT=7,FILE='TRUDATA. DAT',STATUS='NEW') 

OPEN(UNIT=8,FILE='FILDATA. DAT',STATUS='NEW') 

0PEN(UNIT=9,FILE='SMDATA. DAT',STATUS=’NEW’) 

C *** RADIAN/DEGREE CONVERSION FACTORS *** 

RT0D=57. 29577951 
DT0R=0. 01745293 

C *** COMPUTE 4X4 IDENTITY MATRIX *** 

DO 5 1=1,4 
DO 5 J=l,4 
IF (I.EQ.J) THEN 

IMAT(I,J)=l. 0 

ELSE 

IMAT( I, J)=0. 0 

END IF 

5 CONTINUE 

C *** INITIALIZE TIME AND MANEUVER DETECTION ALGORITHM COUNTERS *** 

TIMEM1=0 

NP=1 

TIMEX=5000 


*** COMPUTE BEARING MEASUREMENT COVARIANCE *** 
BEARING ERROR STANDARD DEVIATION = 3 DEGREES 

R=(3*DTOR)**2 


* THIS WHERE THE EXTENDED KALMAN FILTERING STARTS 


*** READ IN OBSERVATION PACKET (TIME, if OF SENSORS) *** 
DT=TIME(K)-TIME(K-l) 

WRITEC*,*)'EXTENDED KALMAN FILTERING NOW STARTS' 
WRITE(*,*) '** * - i -- - ir k * ' 

810 READ(2,1001,END=800)TIME,XT,YT,XS(1),YS(1),DBRG(1), 

* XS(2),YS(2),DBRG(2) 

1001 FORMAT(I4,8F9. 4) 

BD=0.0 
MC=0.0 

DC 200 L=1,2 

IF (DBRG(L).GT. 180.0) DBRG(L)=DBRG(L)-360 
BRG(L)=DBRG(L)*DTOR 
200 CONTINUE 

IF (TIME. LT. 0) GOTO 800 
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DT=TIME“TIMEM1 
DTS(NP)=DT 

CALL FINDPHI(PHI,DT) 

XS1=XS(1) 

YS1=YS(1) 

XS2=XS(2) 

YS2=YS(2) 

BRG1=BRG(1) 

BRG2=BRG(2) 

CALL MP(XS1,YS1,XS2,YS2,BRG1,BRG2,ZX,ZY) 
IF(TIME.EQ.O) THEN 

CALL INIT(XS1,YS1,XS2,YS2,BRG1,BRG2,XKK,PKK) 

ENDIF 

*** PROJECT AHEAD STATE ESTIMATES *** 

X(K+1/K) = PHI * X(K/K) 

CALL MATMULC PHI,XKK,4,4,1,XKKM1) 

*** DERIVATION OF THE Q MATRIX *** 

CALL GETQCDT.XKKMl.PKKCl.D^KKO.S)^) 

*** PROJECT AHEAD ERROR COVARIANCE ESTIMATES *** 

PCK+l/K) = (PHI * P(K/K) * PHIT) + Q 

CALL MATRAN(PHI,PHIT,4,4) 

CALL MATMUL(PHI,PKK,4,4,4,TEMP6) 

CALL MATMULCTEMP6,PHIT,4,4,4,TEMP4) 

301 CALL MATADD(TEMP4,Q,4,4,1,PKKM1) 

C *** 

IF (TIME. EQ. 0) THEN 
DO 542 1=1,4 
DO 542 J=i,4 
PKKM1(I,J)=0.0 

542 CONTINUE 

PKKM1(1,1)=10000. 0 
PKKM1(3,3)=9999.9 
PKKM1(2,2)=0. 25 
PKKM1(4,4)=PKKM1(2,2) 

DO 543 1=1,4 
DO 543 J=1,4 
SPKKM1(I,J)=PKKM1(I,J) 

543 CONTINUE 
ENDIF 

IF (MC.EQ. 1.0) GOTO 303 
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*** CALCULATE THE RESIDUAL DUE TO THE DIFFERENCE BETWEEN 
OBSERVED AND ESTIMATED POSITIONS *** 

| Z(K) - XCK/K-l) | \ 

PX=ZX-XKKM1(1,1) 

PY=ZY-XKKM1(3,1) 

PD=(PX**2)+(PY**2) 

PDIFF=SQRT(PD) 

*** CALCULATE THE MANEUVER GATE THRESHOLD VALUES *** 

CALL MANDET(TIME,PDIFF,XKKM1(1,1),XKKM1(3,1),PKKM1(1,1), 

* PKKM1(3,3),PKKM1(1,3),XPL,YPL,XPU,YPU,TL,TU) 

DO 640 IE=1,37 

WRITEC4,*)XPL(IE),YPL(IE),XPU(IE),YPU(IE) 

►0 CONTINUE 

*** STORE THE MANEUVER GATE VALUES AND RESIDUAL DUE TO THE 
POSITION DIFFERENCE *** 

TLS(NP)=TL 
TUS(NP)=TU 
DR(NP)=PDIFF 

C *** MANEUVER DETECTION/DIVERGENCE ALGORITHM *** 

IF ((PDIFF. GE. TL).AND.(PDIFF. LE. TU)) THEN 
WRITEC*,*)’MANEUVER POSSIBILITY’ 

MC=1. 0 
ZT=ZT+1 

IF (ZT.GE. 2.0) THEN 

IF (TIMEX. GT. TIME) THEN 
TIMEX=TIME 
TIMEXB(NP)=TIME 

TIMEXLO(NP)=TIMEXB(NP)-(1*DTS(NP)) 

WRITE(5,1042)TIMEXB(NP).TIMEXLO(NP) 

F0RMAT(2I4) 

ENDIF 

CALL MATSCL(2.0,Q,4,4,Q) 

TIMEP(NP)=TIME 
WRITEC6,1048)TIMEP(NP) 

FORMAT(14) 

GOTO 301 
ENDIF 
ELSE 

ZT=0.0 
TIMEX=5000 
ENDIF 

C *** RECOGNAZITION OF THE BAD OBSERVATIONS *** 

IF (PDIFF. GE.TU) THEN 
IF (BOC.NE. 1. 0)THEN 
BD=1. 0 
B0C=1. 0 


1042 


1048 
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DO O O 


ELSE 

B0C=0. 0 
END IF 
ELSE 

B0C=0.0 
ENDIF 


303 MC=0.0 

C *** 

IF (TIME. EQ. 0. 0) THEN 
DO 544 1=1,4 
DO 544 J=1,4 
PKKM1(I,J)=SPKKM1(I,J) 
544 CONTINUE 

ENDIF 


204 CONTINUE 

DO 210 L=l,2 

C *** CALCULATE RANGE TO TARGET *** 

XDIFF=XKKM1(l.l)-XS(L) 

YDIFF=XKKM1(3,1)-YS(L) 
RANGE=SQRT(XDIFF**2+YDIFF**2) 

*** UPDATE H MATRIX WITH LATEST STATE ESTIMATES *** 
AND CALCULATE MEASUREMENT ERROR *** 

H(1,1)=YDIFF/RANGE**2 
H(1,2)=0. 0 

H(1,3)=-XDIFF/RANGE**2 
H(l,4)=0.0 

BRKKM1=ATAN2(XDIFF,YDIFF) 

E(L)=BRG(L)-BRKKM1 

*** COMPUTE KALMAN GAIN MATRIX *** 
G=PKKM1*HT*(H*PKKM1*HT+R)**(-1) 

CALL MATRAN(H,HT,1,4) 

CALL MATMUL(H,PKKM1,1,4,4,TEMPI) 

CALL MATMULCTEMP1,HT,1,4,1,TEMP2) 
VARE(L)=TEMP2(1,1)+R 
FAC1=1/VARE(L) 

CALL MATMUL(PKKM1,HT,4,4,1.TEMP3) 

CALL MATSCL(FAC1,TEMP3,4,1,G) 

C *** COMPANSATION OF THE BAD OBSERVATIONS *** 

IF (BD.EQ. 1.0) THEN 
DO 730 1=1,4 
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G(I,1)=0.0 
730 CONTINUE 

ENDIF 

C *•** 

IF (L.EQ. 1) THEN 
G11=G(1,1) 

G13=G(3,1) 

ELSE 

G21=G(1,1) 

G23=G(3,1) 

ENDIF 

*** COMPUTE UPDATED ESTIMATE *** 

XCK/K) = XCK/K-l) + G * E, WHERE E = Z(K) - H(K)*X(K/K-1) 

XKK(1,1)=XKKM1(1,1)+(G(1,1)*E(L)) 

XKK( 2,1)=XKKM1( 2, l)+( G( 2,1)*E( L) ) 

XKK( 3,1)=XKKM1( 3,1)+(G( 3,1)*E(L) ) 
XKK(4,1)=XKKM1(4,1)+(G(4,1)*E(L)) 

*** COMPUTE UPDATED ERROR COVARIANCE MATRIX *** 

P(K/K) = (I - G*H) * P(K/K-1) 

CALL MATMUL(G,H,4,1,4,TEMP4) 

CALL MATSUB(IMAT,TEMP4,4,4,TEMP5) 

CALL MATMUL(TEMP5,PKKM1,4,4,4,PKK) 

*** JF more MEASUREMENTS,*** 

IF (L.LT. 2) THEN 

*** USE UPDATED STATE AND ERROR COVARIANCE ESTIMATES FOR NEXT 
MEASUREMENT *** 

DO 150 1=1,4 
DO 150 J=l,4 

PKKM1(I,J)=PKK(I,J) 

XKKM1(I,1)=XKK(1,1) 

150 CONTINUE 

ENDIF 

210 CONTINUE 


C *** THESE STATEMENTS ARE FOR THE SMOOTHING ALGORITHM *** 

DO 620 1=1,4 
XKKS(I,1,NP)=XKK(1,1) 

620 CONTINUE 


DO 630 1=1,4 
DO 630 J=1,4 

PKKS(I,J,NP)=PKK(I,J) 
630 CONTINUE 
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C *** COMPUTE TRUE TRACKING AND OBSERVATION ERRORS *** 


TRKERR(NP)=SQRT((XT-XKK(1,1))**2+(YT-XKK(3,1))**2) 
OBSERR(NP)=SQRT((XT-ZX)**2+(YT-ZY)**2) 

C *** COMPUTE ESTIMATED X-Y POSITION, COURSE, AND SPEED *** 

XPOS=XKK(1,1) 

YPOS=XKK(3,1) 

IF (XKK(2,1). EQ. 0 .AND. XKK(4,1). EQ. 0) THEN 
HDG=0. 0 

ELSE 

HDG=RT0D*ATAN2(XKK(2,1),XKK(4,1)) 

ENDIF 

IF (HDG. LT. 0.0) HDG=HDG+360 

SPD(NP)=60*SQRT(XKK(2,1)**2+XKK(4,1)**2) 

WRITE(7,1011)TIME, XT,YT 

1011 FORMAT(14,2F15. 4) 

WRITEC 8,1012)TIME,NP,XPOS,YPOS,ZX,ZY,TRKERR(NP),OBSERR(NP), 
* PKK(1,1) 

1012 F0RMAT(2I4,7F15.4) 

C *** UPDATE DATA COUNTER *** 

NP=NP+1 

TIMEM1=TIME 

GOTO 810 


800 NP=NP-1 

Q*** * * **-k*****k ** tt i r k**1rk1rkick ****+1t*** ]' *******i r k* * ** **irk ***- l '**-k/'+**-k** 

C* THIS IS WHERE THE FIXED-INTERVAL SMOOTHING ALGORITHM STARTS * 

A A ' iV"A fr ici c i r i r l ric iri ci rk ic / t+rfc'fc- i e i cIck-ic i c I r l rfcleMr i cMr kici c Jc 

WRITE(*,*)'FIXED-INTERVAL SMOOTHING NOW STARTS' 

WRITE(*,*)' 

DO 3000 KK=1,NP-1 

K=NP-KK 

DT=DTS(K+1) 

TIME=TIMEM1-DT 
CALL FINDPHI(PHI.DT) 

DO 901 1=1,4 
XSS(I,1)=XKKS(I,1,K) 

901 CONTINUE 

DO 902 1=1,4 
DO 902 J=l,4 
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PSS(I,J)=PKKS(I,J,K) 

902 CONTINUE 

C *** CALCULATE THE PREDICTED STATE ESTIMATES *** 

C X(K+1/K)=PHI*X(K/K) 

CALL MATMUL (PHI,XSS,4,4,1,XKKM1S) 

C *** DERIVATION OF THE Q MATRIX *** 

CALL GETQ(DT,XKKM1S,PSS(1,1),PSS(3,3),Q) 

C *** CALCULATION OF THE PREDICTED ERROR COVARIANCE MATRIX 
C AND COMPANSATION ALGORITHM WHICH USES THE MANEUVER 

C PERIOD DETECTED IN THE EXTENDED KALMAN FILTER ROUTINE *** 
C P(K+1/K)=PHI*P(K/K)*PHIT+Q 

CALL MATRAN (PHI,PHIT,4,4) 

CALL MATMULC PHI, PSS, 4,4,4,TEMP6) 

CALL MATMUL(TEMP6,PHIT,4,4,4,TEMP4) 

IF (TIME.EQ. TIMEP(K)) THEN 

IF (TIME. EQ. TIMEP(1)) GOTO 483 
CALL MATSCL(2.0,Q,4,4,Q) 

ELSE 

305 READ(6,1051,END=482)TIMEM,TIMEL 

1051 F0RMAT(2I4) 

IF (TIME. EQ. TIMED THEN 
CALL MATSCL(2.0,Q,4,4,Q) 

END IF 
GOTO 305 
ENDIF 

482 REWIND 6 

483 CALL MATADD(TEMP4,Q,4,4,1,PKKM1S) 

C *** CALCULATE THE SMOOTHING FILTER GAIN MATRIX *** 

C AK=P(K/K)*PHIT*INV # P(K+1/K) 

CALL MATINV (PKKM1S,4,IPKKM1S) 

CALL MATMUL (PHIT,IPKKM1S,4,4,4,TEMP1S) 

CALL MATMUL (PSS,TEMP1S,4,4,4,AK) 


DO 904 1=1,4 

XNNM1CI,1)=XKKS(I,1,K+l) 
904 CONTINUE 


C *** CALCULATE THE SMOOTHED STATE ESTIMATE *** 
C XKKS=X(K/K)+AK*(X(K+l/N)-X(K+l/K) 

CALL MATSUB (XNNM1.XKKM1S,4,1.TEMP2S) 
CALL MATMUL (AK,TEMP2S,4,4,1.TEMP3S) 
CALL MATADD (XSS,TEMP3S,4,1,1,TH1) 

DO 903 1=1,4 
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XKKS(I,1,K)=TH1(I,1) 

903 CONTINUE 

DO 906 1=1,4 
DO 906 J=1,4 

PNNH1(I,J)=PKKS(I,J,K+1) 
906 CONTINUE 


*** CALCULATE THE SMOOTHED COVARIANCE MATRIX *** 
PKKS=P(K/K)+AK*|P(K+1/N)-P(K+1/K)|*AKT 

CALL MATSUB (PNNM1,PKKM1S,4,4,TEMP4S) 

CALL MATRAN (AK,AKT,4,4) 

CALL MATMUL (AK.TEMP4S,4,4,4,TEMP5S) 

CALL MATMUL (TEMP5S,AKT,4,4,4,TEMP6S) 

CALL MATADD (PSS,TEMP6S,4,4,1,TH2) 

DO 908 1=1,4 
DO 908 J=1,4 
PKKS(I,J,K)=TH2(I,J) 

908 CONTINUE 


C *** COMPUTE ESTIMATED X-Y POSITION, COURSE, AND SPEED *** 

IF (XKKS(2,1,K).EQ.0 .AND. XKKS(4,1,K).EQ.0) THEN 
SHDG=0.0 

ELSE 

SHDG=RTOD*ATAN2(XKKS(2,1,K),XKKS(4,1,K)) 

ENDIF 

IF (SHDG.LT.O.O) SHDG=SHDG+360 
SSPD(K)=60*SQRT(XKKS(2,.T,K)**2+XKKS(4,1,K)**2) 

TIMEM1=TIME 

3000 CONTINUE 

REWIND 4 

C *** CALCULATE THE SMOOTHED TRACKING ERROR *** 

DO 1100 K=1,NP 

SXPOS=XKKS(1,1,K) 

SYPOS=XKKS(3,1,K) 

READ(4,1110)TIME,XT, YT 

STRKERR(K)=SQRT( (XT-XKKS( 1,1 ,K) )**2+(YT-XKKS(3,1,K) )**2) 

WRITE(9,1120)K,SXPOS,SYPOS,STRKERR(K),PKKS(1,1,K) 

1100 CONTINUE 

1110 FORMATCI4.2F15. 4) 

1120 FORMATC14,4F20. 4) 

CL0SE(UNIT=2) 

CL0SE(UNIT=3) 
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CL0SE(UNIT=4) 

CL0SE(UNIT=5) 

CL0SE(UNIT=6) 

CL0SE(UNIT=7) 

CL0SE(UNIT=8) 

CL0SE(UNIT=9) 

WRITE(*,*)'THERE WERE',NP,' OBSERVATIONS PROCESSED.’ 

WRITE(*,*)’FOR GRAPHIC RESULTS COPY' 

WRITE(*,*)' 1) FILDATA.DAT* 

WRITE(*,*)’ 2) MANEUDATA.DAT’ 

WRITE(*,*)’ 3) SMDATA.DAT' 

WRITEC*,*)' 4) TRUDATA.DAT’ 

WRITE(*,*)’TO THE MATLAB SUB-DIRECTORY AND RUN => <SHIPTR.M>’ 

STOP 

END 



SUBROU TINE FINDP HI(PHI,DT) 

C COMPUTES THE VALUES OF THE PHI MATRIX 

Q -k-Ai r l c ft •h.-A-fi ArcUnfc ft ir / tiHtlr l tic i r lr/ tir /r k- f cin 

REAL*4 PHI(4,4),DT 

DO 1501 1=1,4 
DO 1501 J=l,4 
DO 1501 K=l,2 

PHI(I,J)=0. 0 

1501 CONTINUE 

C *** COMPUTE PHI MATRIX *** 

DO 1500 1=1,4 
PHI(I,I)=1. 0 
1500 CONTINUE 

PHI(1,2)=DT 
PHI(3,4)=DT 

RETURN 

END 


SUBRO UTINE INIT( XSl,YSl,XS2,YS 2,BRGl ,BRG2,?aCK ; PKK ) 

C THIS ROUTINE INITIALIZES THE STATE 

C A N D ERRO R COVARIAN CE ESTIMATES 

REAL*4 XKK(4,1),PKK(4,4) 

REAL*4 XS1,YS1,XS2,YS2,BRG1,BRG2 
REAL*4 NUMER.DENOM 
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C *** INITIAL STATE ESTIMATE *** 

NUMER=(-YS2*TAN(BRG2))+(YS1*TAN(BRG1))+XS2-XS1 
DENOM=TAN(BRG1)-TAN(BRG2) 

XKK(3,1)=NUMER/DEN0M 
XKK(2,1)=0. 0 

XKK(1,1)=(XKX(3,1)-YS1)*TAN(BRG1)+XS1 
XKK(4,1)=0. 0 

C *** INITIAL ERROR COVARIANCE ESTIMATE *** 

DO 555 1=1,4 
DO 555 J=1,4 
PKK(I,J)=0.0 
555 CONTINUE 

PKK(1,1)=10000. 0 
PKK(3,3)=9999. 9 
PKK(2,2)=0. 25 
PKK(4,4)=PKK(2,2) 

RETURN 

END 


SUBROUTINE GETQ(DT,XKKM1L,P11,P33,Q) 

g* * i t i t *A *J rWA<! <n l r * ** )HHrit fr lr *< r*il( A* * iH l * **** ilr*A A 'j nt il r * **** ** * ** ***A** ' i lr * ' < r *) i t ' ^* 

C CALCULATES STATE EXCITATION MATRIX Q 

Qltirie'/rlriric^rie-i r l rkMrk - Mc/i-kir / i A-k' i r kirki c/e ic/ti r i c^r^ ’k-i r ir/ ti t i VjV A ft ' A ' A r A A AA * " A'AnATfcA n fc' A , A T fc T A l A , A*ft Mrfrk 

REAL*4 DT,XKKM1L(4,1),QT,P11,P33,NT,Q(4,4) 

REAL*4 QM,VT,SIGTHT,SIGVT 

SIGTHT=0. 0001 
SIGVT=0. 0001 

IF ((XKKM1LC2,1). EQ- 0). OR. (XKKM1L(4,1). EQ. 0)) THEN 
QT=0.0 
GOTO 200 
END IF 

VT=SQRT((XKKM1L(2,1)**2)+(XKKM1L(4,1)**2)) 

IF (Pll. GT. P33) THEN 

QT=( ( (XKKM1LC 2, l)/VT)**2)*SIGVT)+( (XKKM1L(4, l)**2)*SIGTlfr) 
ELSE 

QT=(((XKKM1L(4,l)/VT)**2)*SIGVT)+((XKKM1L(2,1)**2)*SIGTHT) 
ENDIF 

200 NT=(DT**4)/4.0 

QM=NT*QT 

DO 556 1=1,4 
DO 556 J=1,4 
Q(I,J)=0. 0 
556 CONTINUE 
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557 


DO 557 1=1,4 
Q(I,I)=QM 
CONTINUE 

CALL MATSCLC 0. 1,Q,4,4,Q) 

RETURN 

END 


SUBROUTINE MP(XS1,YS1,XS 2,YS 2,BRG1,BRG2,ZX,ZY) 

THIS ROUTINE COMPUTES THE OBSERVED X, Y POSITIONS OF THE 
TARGET USING SENSOR SHIP POSITIONS AND BEARINGS TO THE TARGET 

Itltlrje& '/ r h M cj c k-I r l r itl c i r ici r ki c / c / c i r ici f / r i r itJ f f r /r Mc -l r Airirit M tjc It A A /c' A ' A j 1 ; A A ' i\ A " A A 'l V ' A A h A ■MeMririrMt 

REAL*4 ZX,ZY 

REAL*4 XS1,YS1,XS2,YS2,BRG1,BRG2 
REAL*4 NUMER.DENOM 

NUMER=C-YS2*TAN(BRG2))+(YS1*TAN(BRG1))+XS2-XS1 
DENOM=TAN(BRG1)-TAN(BRG2) 

ZY=NUMER/DENOM 
ZX=(ZY-YS1)*TAN(BRG1)+XS1 

RETURN 

END 

SUBROUTINE MANDET(TIME,DIFF,XT,YT,P1,P3,P13,XPL,YPL, 

* XPU,YPU,TL,TU) 

•kirfci c Mc-lcfrlricirk ir l i ■ /c/ c J r f cir i t m trf c fr-/c^ r trfrt : -/ c it'i r i c ic - lei r f r jiie-/r1r1r/ r /riririrk-iriri r fri r 1t1eit AAA A 

THIS SUBROUTINE COMPUTES THE THRESHOLD VALUES OF THE 
MANEUVER GATES USING ERROR ELLIPSE EQUATIONS 

i r l rMrk i rMtis-i e -!e1r it i: -/rlr/ei tirir k-i cirMeir /r/ r/c- /e ii!itr l r.’ e ir lr *' / f /r/ eir i eie- /rf r / r / cA * *** * * * A A ' * 

REAL*4 XT,YT,XPL(21),YPL(21),XPU(21),YPU(21),THE1,SIG2X 
REAL*4 SX,SY,CT,P1,P13,P3,DIFF,TL,TU,C,D,DT0R,A,B,SIG2Y 
REAL*4 THETA,DIV 

INTEGERS NP,TIME,CO 

DT0R=0. 0174529 
DIV=30. 0 
A=2*P13 
B=P1-P3 

THE1=0.5*ATAN2(A,B) 

C=(Pl+P3)/2 
D=0. 0 

IF (P13.EQ. 0. 0) GOTO 10 
D=P13/SIN(2. 0*THE1) 

10 SIG2X=ABS(C+D) 

SIG2Y=ABS(C-D) 

SX=(SIG2X**0.5) 

SY=(SIG2Y**0.5) 

IF (SX.GT.SY) THEN 
TL=3*SX 
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TU=8*SX 

ELSE 

TL=3*SY 

TU=8*SY 

ENDIF 

CT=C0S(THE1) 

ST=SIN(THE1) 

IF (TIME. GT. 0) THEN 
TL=TL/DIV 
TU=TU/DIV 
ENDIF 

WRITE(3,1045)TIME,TL,TU,DIFF 
1045 FORMAT(I4,3F10.4) 

DO 100 IE=1,37 
CO=IE-l 

THETA=((360/36)*C0)*DT0R 
XPU(IE)=TU*COS(THETA)+XT 
YPU(IE)=TU*SIN(THETA)+YT 

100 CONTINUE 

DO 120 IE=1,37 
CO=IE-l 

THETA=((360/36)*C0)*DT0R 
XPL(IE)=TL*COS(THETA)+XT 
YPL(IE)=TL*SIN(THETA)+YT 

120 CONTINUE 

RETURN 

END 


SUBROUTINE MATMUL(A,B,L,M,N,C) 


C THIS ROUTINE MULTIPLIES TWO MATRICES TOGETHER 

C M ’ A A 

REAL*4 A(L,M),B(M,N),C(L,N) 

DO 10 1=1,L 
DO 10 J=1,N 
C(I,J)=0.0 
10 CONTINUE 

DO 100 1= 1,L 
DO 100 J= 1,N 
DO 100 K= 1,M 

C(I,J) = C(I,J) + A(I,K)*B(K,J) 

100 CONTINUE 

RETURN 

END 
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SUBROUTINE MATRAN(A,B,N,M) 


THIS ROUTINE TRANSPOSES A MATRIX 
0 B(M,N) = A'(N,M) 


REAL*4 A(N,M), B(M,N) 

DO 100 1= 1,N 
DO 100 J= 1,M 
B(J,I) = A(I,J) 

100 CONTINUE 

RETURN 

END 


SUBROUTINE MATSCL(SC,A,N,M,C) 


THIS ROUTINE MULTIPLIES A MATRIX WITH A SCALAR 
0 C(N,M) = SC * A(N,M) 


REAL*4 A(N,M), C(N,M), SC 

DO 100 I = 1,N 
DO 100 J = 1,M 
C(I,J) = SC*A(I,J) 

100 CONTINUE 

RETURN 

END 


SUBROUTINE MATSUB(A,B,N,M,C) 


THIS ROUTINE SUBTRACTS TWO MATRICES 
# C(N,M) = A(N,M) - B(N,M) 


REAL*4 A(N,M),B(N,M),C(N,M) 

DO 100 I = 1,N 
DO 100 J = 1,M 
C( I, J)=A( I, J) “B( I, J) 

100 CONTINUE 

RETURN 

END 

SUBROUTINE MATADD(A,B,N,M,L,C) 


THIS ROUTINE ADDS TWO MATRICES 
0 C(N,M) = A(N,M) + B(N,M) 
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REAL*4 A(N,M),B(N,M),C(N,M,L) 
DO 100 I = l,N 
DO 100 J = 1,M 
C(I,J,L)=A(I,J)+B(I,J) 

100 CONTINUE 

RETURN 

END 


SUBROUTINE MATINV (A,N,C) 


C THIS ROUTINE COMPUTES THE INVERSE OF 

C A MATRIX 

C C(N,N)=INV |A(N,N)| 


REAL*4 A(N,N),C(N,N),D(20,20) 
DO 100 I = 1,N 
DO 100 J = 1,N 
100 D(I,J)=A(I,J) 


DO 115 1=1,N 
DO 115 J=N+1,2*N 
115 D(I,J)=0.0 


DO 120 1=1,N 
J=I+N 

120 D(I,J)=l.0 

DO 240 K=1,N 
M=K+1 

IF (K.EQ.N) GOTO 180 
L=K 

DO 140 I=M,N 

140 IF (ABS(D(I,K)).GT. ABS(D(L,K))) L=I 

IF (L.EQ.K) GOTO 180 

DO 160 J=K,2*N 
TEMP=D(K,J) 

D(K,J)=D(L,J) 

160 D(L,J)=^rEMP 


180 DO 185 J=M,2*N 

185 D(K,J)=D(K,J)/D(K,K) 

IF (K.EQ. 1) GOTO 220 
M1=K-1 

DO 200 1=1,Ml 
DO 200 J=M,2*N 

200 D(I,J)=D(I,J)-D(I,K)*D(K,J) 

IF (K.EQ.N) GOTO 260 

220 DO 240 I=M,N 

DO 240 J=M,2*N 

240 D(I,J)=D(I,J)-D(I,K)*D(K,J) 
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260 

DO 265 1=1,N 


DO 265 J=1,N 


K=J+N 

265 

C(I,J)=D(I,K) 


RETURN 


END 
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APPENDIX B. INPUT DATA FILE FORMATTING ALGORITHM 

C *** RAWDATA. FOR *** 


C* 


* 


C* THIS PROGRAM EMPLOYES A TARGET AND TWO SENSOR SHIPS. IT ASKS FOR * 
C* THE INITIAL POSITIONS, SPEEDS AND COURSES OF THE TARGET AND SENSOR * 
C* SHIPS. IT ALSO CALLS FOR MANEUVER PERIOD, THE SPEED AND THE COURSE * 
C* CHANGE OF THE TARGET DURING THIS MANEUVER PERIOD. THE OUTPUTS * 
C* WHICH CONSIST OF NOISY OR NOISE FREE BEARINGS FROM THE SENSOR SHIPS * 
C* TO THE TARGET AND POSITIONS OF THE EACH SHIPS ARE STORED IN THE FILE* 
C* TRKDATA. DAT TO BE USED BY THE PROGRAM <SHIPM ANE. F OR>. * 


C *** VARIABLE DEFLATIONS *** 


C BRG 

C CASE 

C 

C CS, CE 

C DTOR 

C END 

C HDGS 

C HDGT 

C HDGTD 

C MS, ME 

C Nl, N2 

C NM 

C PER 

C RTOD 

C SS, SE 

C SPDS 

C SPDT 

C SPDTD 

C TIMED 

C UNHDGCH 

C UNSPDCH 

C XDIFF, YDIFF 

C 

C XS 

C XT 


MEASURED TARGET BEARINGS. 

INDICATOR OF THE NOISE EXISTANCE. THE NOISE EXISTS 
FOR POSITIVE VALUE, NO NOISE FOR NEGATIVE VALUE. 
START AND END HEADINGS OF THE MANEUVER. 

DEGREE TO RADIAN CONVERSITION FACTOR. 

END OF THE TRACKING PROBLEM. 

SENSOR SHIP’S HEADING. 

TARGET'S HEADING. 

TOTAL HEADING CHANGE DURING THE MANEUVER. 

START AND END TIMES OF THE MANEUVER. 

MEASUREMENT NOISESS. 

NUMBER OF MANEUVERS. 

OBSERVATION PERIOD. 

RADIAN TO DEGREE CONVERSITION FACTOR. 

START AND END TIMES OF THE MANEUVER. 

SENSOR SHIP'S SPEED. 

TARGET'S SPEED. 

TOTAL SPEED CHANGE DURING THE MANEUVER. 

TOTAL MANEUVER TIME. 

HEADING CHANGE PER OBSERVATION. 

SPEED CHANCE PER OBSERVATION. 

THE DISTANCES IN THE X AND Y DIRECTIONS FROM SENSOR 
TO A TARGET POSITION. 

SENSOR SHIP'S STATES. 

TARGET’S STATES. 


C *** VARIABLE DECLERATIONS *** 

REAL*4 XT(4,1),XS1(4,1),PHI(4,4),SPDS1,HDGS1,SPDS2,HDGS2,SP,HD 
REAL*4 DT,SPDT,HDGT,XS2(4,1),TEMP1(4,1),CASE,XDIFF1,YDIFF1 
REAL*4 XDIFF2,YDIFF2,N1,N2,DTOR,RTOD,BRG1,BRG2,CS,CE(20),HDGTD 
REAL*4 MS(20),ME(20),SS,SE(20),SPDTD,UNSPDCH(10),UNHDGCH(10) 


INTEGER TIME,TIMEMl.NM,PER,END 
C *** OPEN DATA FILES *** 
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0PEN(UNIT=2,FILE='NOISE 1. DAT' ,STATUS='OLD' ) 
0PEN(UNIT=3,FILE='NOISE2.DAT'.STATUS='OLD') 
OPEN(UNIT=4,FILE='TRKDATA. DAT f ,STATUS=’NEW') 


WRITEC*,*)'ENTER A NEGATIVE NUMBER FOR NOISELESS CASE;' 
WRITEC*,*)'POSITIVE FOR NOISY CASE' 

READC*,*)CASE 

TIMEM1=0 

RTOD=57. 29577951 
DT0R=0. 017453293 

WRITEC*,*)'ENTER THE OBSERVATION PERIOD AND' 

WRITEC*,*)'END OF THE OBSERVATION TIME. ' 

READC*,*)PER,END 

WRITEC*,*)'INPUT DESIRED INITIAL X POSITION, Y POSITION,' 
WRITEC*,*)'SPEED CIN KNOTS) AND COURSE CIN DEGREES) OF TARGET' 
READC*,*)XTC1,1), XTC3,1),SPDT,HDGT 

SP=SPDT 

HD=HDGT 

XTC2,1)=CSPDT/60)*SINCHDGT*DTOR) 

XTC 4,1)=CSPDT/60)*COS C HDGT*DTOR) 

WRITEC*,*)'FOR SENSOR 1:' 

WRITEC*,*)'INPUT DESIRED INITIAL X POSITION, Y POSITION,' 
WRITEC*,*)'SPEED CIN KNOTS) AND COURSE CIN DEGREES)' 
READC*,*)XS1C1,1),XS1C3,1),SPDS1,HDGS1 

XS1C2,1)=CSPDS1/60)*SINCHDGS1*DT0R) 

XS1C 4,1)=C SPDS1/60)*COS C HDGS1*DT0R) 

WRITEC*,*)'FOR SENSOR 2: ' 

WRITE(*,*)'INPUT DESIRED INITIAL X POSITION, Y POSITION,' 
WRITEC*,*)'SPEED CIN KNOTS) AND COURSE CIN DEGREES)' 

READC*,*)XS2C1,1),XS2(3,1),SPDS2,HDGS2 

XS2C 2,1)=CSPDS2/60)*SINCHDGS2*DTOR) 

XS2C 4,1)=C SPDS2/60)*C0SC HDGS2*DT0R) 

WRITEC*,*)'HOW MANY TIMES DO YOU WANT TO MAKE MANEUVER?’ 
READC*,*)NM 

DO 540 K=1.NM 
WRITEC*,*) ' 

WRITEC*,*)'**MANEUVER #',K 

510 WRITEC*,*)'ENTER THE STARTING AND ENDING TIMES OF* 

WRITEC*,*)'THE MANEUVER #',K 
READC*,*)MSCK),MECK) 

IF CCMSCK). GT.END).OR. CMECK). GT. END)) THEN 
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WRITEC*,*)'CAREFULL! END OF THE TRACKING IS',END 
WRITEC*,*)' ' 

GOTO 510 
END IF 

TIMED=ME(K)-MS(K) 

520 WRITEC*,*)'ENTER THE STARTING AND ENDING SPEEDS OF* 

WRITE(*,*)'THE SPEED MANEUVER ,K 
READ(*,*)SS,SE(K) 

IF (SS.NE.SP) THEN 

WRITEC*,*)'CAREFULL! CURRENT SPEED IS',SP 
WRITEC*,*)' ' 

GOTO 520 
END IF 

SP=SECK) 

SPDTD=SECK)-SS 

UNSPDCHCK)=C SPDTD/TIMED)*PER 

530 WRITEC*,*)'ENTER THE STARTING AND ENDING COURSES OF' 

WRITEC*,*)'THE COURSE MANEUVER #',K 
READC*,*)CS,CECK) 

IF CCS. NE. HD) THEN 

WRITEC*,*)'CAREFUL! CURRENT HEADING IS',HD 
WRITEC*,*)' ' 

GOTO 530 
ENDIF 

HD=CECK) 

HDGTD=CECK)-CS 

UNHDGCHC K)=C HDGTD/TIMED)*PER 

540 CONTINUE 

DO 610 J=l, 1000 
DO 550 L=1,NM 

IF CCTIME.GT.CMSCL))).AND. CTIME.LE. CMECL)))) THEN 

SPDT=SPDT+UNSPDCHC L) 

HDGT=HDGT+UNHDGCH(L) 

IF CCUNSPDCHCL). LT. 0. 0). AND. CSPDT. LT.SECL))) SPDT^SECL) 
IF CCUNSPDCHCL).GT.0. 0). AND. CSPDT. GT. SECL))) SPDT=SECL) 

IF CCUNHDGCHCL). LT. 0. 0). AND. CHDGT.LT. CECL))) HDGT=CECL) 
IF CCUNHDGCHCL).GT. 0.0). AND. CHDGT.GT. CECL))) HDGT=CECL) 

XTC 2,1)=C SPDT/60)*SINC HDGT*DTOR) 

XTC 4,1)=( SPDT/ 60 ) *COS C HDGT^'DTOR ) 

ENDIF 
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S50 CONTINUE 

C **★ UPDATE TARGET AND SENSOR STATES TO MEASUREMENT TIME *** 
DT=TIME-TIMEM1 


C *** COMPUTE PHI MATRIX *** 

CALL FINDPHI(DT,PHI) 

C *** UPDATE TARGET STATES *** 

CALL MATMUL(PHI,XT,4,4,1,TEMPI) 
DO 560 1=1,4 

XT(I,1)=TEMP1(I,1) 

560 CONTINUE 

C *** UPDATE SENSOR STATES *** 

CALL MATMUL(PHI,XS1,4,4,1,TEMP1) 
DO 570 1=1,4 

XS1(I,1)=TEMP1(I,1) 

570 CONTINUE 

CALL MATMUL(PHI,XS2,4,4,IjTEMPl) 
DO 580 1=1,4 

XS2(I,1)=TEMP1(1,1) 

580 CONTINUE 

XDIFF1=XT(1,1)-XS1(1,1) 

YDIFF1=XT(3,1)-XS1(3,1) 

XDIFF2=XT(1,1)-XS2(1,1) 
YDIFF2=XT(3,1)-XS2(3,1) 

READ(2,*)N1 

READ(3,*)N2 

IF (CASE.GE. 0.0) GOTO 590 
N1=0.0 
N2=0.0 


590 


* 

600 


BRG1=RT0D*ATAN2(XDIFF1,YDIFF1)+N1 
IF (BRG1.LT. 0. 0) BRGl=BRGl+360 
BRG2=RT0D*ATAN2(XDIFF2,YDIFF2)+N2 
IF (BRG2.LT. 0. 0) BRG2=BRG2+360 


WRITE(4,600)TIME,XT(1,1),XT(3,1),XS1(1,1),XS1(3,1), 

BRG1,XS2(1,1),XS2(3,1),BRG2 
FORMATCI4,8F9. 4) 


TIMEM1=TIME 

TIME=TIME+PER 


IF (TIME. GT.END) GOTO 620 



o o n o o 


610 


CONTINUE 


620 STOP 
END 


SUBROUTINE FINDPHI(DT.PHI) 



C DIMENSIONS AND DECLERATIONS 

REAL*4 PHI(4,4),DT 


PHI(1,1)=1. 0 
PHI(1,2)=DT 
PHI(1,3)=0. 0 
PHI(1,4)=0. 0 
PHI(2,1)=0. 0 
PHI(2,2)=1. 0 
PHI(2,3)=0. 0 
PHI(2,4)=0. 0 
PHI(3,1)=0. 0 
PHI(3,2)=0. 0 
PHI(3,3)=1. 0 
PHI(3,4)=DT 
PHI(4,1)=0.0 
PHI(4,2)=0. 0 
PHI(4,3)=0. 0 
PHI(4,4)=1. 0 

RETURN 

END 

SUBROUTINE MATMUL(A,B,L,M,N,C) 

THIS ROUTINE MULTIPLIES TWO MATRICES TOGETHER 
0 C(L,N) = A(L,M) * B(M,N) 

DIMENSIONS AND DECLARATIONS 
REAL*4 A(L,M),B(M,N),C(L,N) 

DO 10 1=1,L 
DO 10 J=1,N 
C(I,J)=0. 0 
10 CONTINUE 

DO 100 1= 1,L 
DO 100 J= 1,N 
DO 100 K= l.M 

C(I,J) = C(I,J) + A(I,K)*B(K,J) 

100 CONTINUE 

RETURN 

END 
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